#!/usr/bin/env python3

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, TimerAction
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    """启动完整的自动飞行系统（使用简单z轴控制器）"""
    
    return LaunchDescription([
        # 声明参数
        DeclareLaunchArgument(
            'z_velocity',
            default_value='0.2',
            description='固定z轴速度 (m/s)'
        ),
        
        DeclareLaunchArgument(
            'altitude_tolerance',
            default_value='0.05',
            description='高度到达容差 (m)'
        ),
        
        DeclareLaunchArgument(
            'auto_start_delay',
            default_value='5.0',
            description='自动任务启动延迟时间 (s)'
        ),
        
        # 简单z轴速度控制器
        Node(
            package='pb2025_sentry_nav',
            executable='simple_z_velocity_controller.py',
            name='simple_z_velocity_controller',
            output='screen',
            parameters=[{
                'z_velocity': LaunchConfiguration('z_velocity'),
                'altitude_tolerance': LaunchConfiguration('altitude_tolerance'),
            }]
        ),
        
        # 延迟启动自动飞行任务
        TimerAction(
            period=LaunchConfiguration('auto_start_delay'),
            actions=[
                Node(
                    package='pb2025_sentry_nav',
                    executable='auto_flight_mission.py',
                    name='auto_flight_mission',
                    output='screen'
                )
            ]
        ),
    ]) 